function ret = f_vector(t, x, p)
a = getrobotparameter('a');
ndof = length(x)/2;
dq = x(ndof+1:end);

M    = D_mat(x);
C    = C_mat(x);
G    = G_vec(x);

% Outputs
y_a1     = ya1_sca(x, p, a);
y_a2     = ya2_vec(x, p, a);
y_d1     = yd1_sca(x, p, a);
y_d2     = yd2_vec(x, p, a);

y1 = y_a1 - y_d1;
y2 = y_a2 - y_d2;

% y_a = [y_a1; y_a2];
% y_d = [y_d1; y_d2];

% Jacobians of Outputs
Dy_a1 = Dya1_mat(x, p, a);
Dy_d1 = Dyd1_mat(x, p, a);
Dy_a2 = Dya2_mat(x, p, a);
Dy_d2 = Dyd2_mat(x, p, a);

Dy_1 = Dy_a1 - Dy_d1;
Dy_2 = Dy_a2 - Dy_d2;

% Control Fields
vf    = [dq; M \ (-C*dq - G)];
B_IO  = eye(ndof);
gf    = [zeros(size(B_IO)); M \ B_IO];

% Lie Derivatives
% Lgy1 = Dy_1*gf;
% Lgy2 = Dy_2*gf;
% Lfy1 = Dy_1*vf;
Lfy2 = Dy_2*vf;

% Second Order Jacobians

% DLfy_a2 = DLfya2_mat(x, p, a);
% DLfy_d2 = DLfyd2_mat(x, p, a);

% DLfy2 = DLfy_a2 - DLfy_d2;

% PD control
B_IO  = [1 0 0 0 0;
         0 1 0 0 0;
         0 0 1 0 0;
         0 0 0 1 0;
         0 0 0 0 1];
gf    = [zeros(size(B_IO)); M \ B_IO];

kp = 30;
kd = 10;
u_pd = [kd*y1;
    -kp*y2(2) - kd*Lfy2(2);
    -kp*y2(4) - kd*Lfy2(4);
    -kp*y2(1) - kd*Lfy2(1);
    -kp*y2(3) - kd*Lfy2(3)];
vf = vf + gf * u_pd;

% B_IO  = [0;
%          0;
%          0;
%          1;
%          0];
% gf    = [zeros(size(B_IO)); M \ B_IO];

% Lgy1 = Dy_1*gf;
% LgLfy2 = DLfy2*gf;\
% LfLfy2 = DLfy2*vf;

% Lfy1 = Dy_1*vf;
% Lfy2 = Dy_2*vf;

% A = [LgLfy2(1, :)];

% u = -A \ (LfLfy2(1) + 2*ep*Lfy2(1) + ep^2*y2(1));

[t y1 y2(1) y2(2) y2(3) y2(4)];
ret = vf;

end
